package com.hitqz.robot.driver;

import cn.hutool.core.util.StrUtil;
import com.alibaba.fastjson.JSONObject;
import com.hitqz.robot.api.business.enums.FormConfigEnum;
import com.hitqz.robot.api.business.enums.RobotDriverEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.business.enums.RobotParamsFormFieldEnum;
import com.hitqz.robot.api.common.dto.form.FormConfigDto;
import com.hitqz.robot.api.common.dto.form.FormFieldConfigDto;
import com.hitqz.robot.api.common.dto.robot.InitPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.dto.GfRelayStatus;
import com.hitqz.robot.driver.dto.GfRobotStatus;
import com.hitqz.robot.driver.dto.robot.RobotCustomInfo;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.MapAttributesDto;
import com.hitqz.robot.driver.model.PointDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.driver.util.GfCacheUtil;
import com.hitqz.robot.driver.util.GfToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.text.DecimalFormat;
import java.util.*;

@Slf4j
@Service("robotDriverService")
public class HitGfRobotDriverImpl implements RobotDriverService {
    //驱动电机web控制速度 r/min
    private static short speed = 800;
    // r/min
    private static final short minSpeed = 500;
    // r/min
    private static final short maxSpeed = 1700;

    @Override
    public boolean doOnlineMode(Robot robot) {
        boolean result = false;
        try {
            Integer port = null;
            if (robot.getIpAddress().contains(":")) {
                port = Integer.parseInt(robot.getIpAddress().split(":")[1]);
            }
            GfToolkit.serialNumber = robot.getRobotSn();
            result = GfToolkit.init(port);
        } catch (Exception e) {
            log.error("[gf] doOnlineMode error", e);
        }
        log.info("[gf] doOnlineMode success");
        return result;
    }

    @Override
    public boolean setSpeed(RobotMoveDto baseSpeedDto) {
        //改变标志位
        GfCacheUtil.setSpeedFlag();
        if (StrUtil.isNotBlank(baseSpeedDto.getLinerSpeedY())) {
            int qdMotorSpeed = baseSpeedDto.getLinerSpeedY().contains("-") ? -1 * speed : speed;
            qdMotorSpeed = Objects.equals(baseSpeedDto.getLinerSpeedY(),"0") ? 0 :qdMotorSpeed;
            int gsMotorSpeed = Objects.equals(baseSpeedDto.getLinerSpeedY(),"0") ? 0 :1500;
            GfToolkit.setMotorSpeed( qdMotorSpeed, gsMotorSpeed);
        }
        return true;
    }

    @Override
    public boolean changeWebControlSpeed(float changeValue) {
        //changeValue 是 m/s 需要换算成 cm/s
        short changeValueShort = (short) (changeValue * 1000);
        speed = (short) Math.min(Math.max(speed + changeValueShort, minSpeed), maxSpeed);
        return true;
    }

    @Override
    public boolean setInitPos(InitPosDto baseInitPosDto) {
        return false;
    }

    @Override
    public boolean robotTaskGoTarget(RobotGoTargetPosDto dto) {
        return false;
    }

    @Override
    public boolean motionAbort() {
        return true;
    }

    @Override
    public boolean navCmd(Integer cmd) {
        return false;
    }

    @Override
    public R executeOtherCommand(RobotOtherCommandEnum cmdEnum,String jsonParams) {
        if (cmdEnum == RobotOtherCommandEnum.SET_WORK_MODE) {
            Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
            //GfToolkit.setWorkMode(
            //        (byte) Integer.parseInt(String.valueOf(params.get("workMode"))),
            //        (byte) Integer.parseInt(String.valueOf(params.get("workNum"))));
            GfToolkit.setWork(
                    (byte) Integer.parseInt(String.valueOf(params.get("workMode"))),
                    Integer.parseInt(String.valueOf(params.get("qdMotorSpeed"))),
                    Integer.parseInt(String.valueOf(params.get("gsMotorSpeed"))),
                    (byte) Integer.parseInt(String.valueOf(params.get("workNum")))
            );
        }
        if (cmdEnum == RobotOtherCommandEnum.SET_GF_FUNCTION){
            Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
            GfToolkit.setFunctionEnable(
                    (byte) Integer.parseInt(String.valueOf(params.get("function"))),
                    (byte) Integer.parseInt(String.valueOf(params.get("enable")))
            );
        }
        if (cmdEnum == RobotOtherCommandEnum.SET_GF_IO){
            Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
            GfToolkit.setIoEnable(
                    (byte) Integer.parseInt(String.valueOf(params.get("number"))),
                    (byte) Integer.parseInt(String.valueOf(params.get("enable")))
            );
        }
        if (cmdEnum == RobotOtherCommandEnum.SET_GF_RELAY){
            Map<String, Object> params = JSONObject.parseObject(jsonParams, Map.class);
            GfToolkit.setRelayStatus(
                    (byte) Integer.parseInt(String.valueOf(params.get("number"))),
                    (byte) Integer.parseInt(String.valueOf(params.get("enable")))
            );
        }
        return null;
    }

    @Override
    public IRobotPosDto getWebPos() {
        IRobotPosDto iRobotPosDto = new IRobotPosDto();
        GfRelayStatus relayStatus =(GfRelayStatus) GfCacheUtil.getLongtimeCache(GfCacheUtil.KEY_ROBOT_STATUS);
        if (relayStatus!=null){
            iRobotPosDto.setCurrentDirection("待机");
        }
        GfRobotStatus status = (GfRobotStatus) GfCacheUtil.get(GfCacheUtil.KEY_ROBOT_STATUS);
        if (status!=null){
            iRobotPosDto.setControllerTemp(status.getChipTemperature());
            iRobotPosDto.setBlocked(false);
            iRobotPosDto.setBatteryLevel(status.getPower());
            iRobotPosDto.setCharging(status.getCurrentDirection()==1);
            iRobotPosDto.setVoltage(status.getVoltage());
            iRobotPosDto.setCurrent(String.valueOf(status.getCurrent()));
            DecimalFormat df = new DecimalFormat("0.###");
            iRobotPosDto.setSpeed(Double.parseDouble(df.format(status.getRobotSpeed())));
            iRobotPosDto.setCurrentDirection(status.getCurrentDirection() == 0 ? "放电": "充电");
            iRobotPosDto.setTemperature(status.getTemperature());
            iRobotPosDto.setX(status.getMileage());
            iRobotPosDto.setCurrentMap("default");
        }
        return iRobotPosDto;
    }

    @Override
    public List<RobotMapPos> getNavPath(RobotMapPos sourcePos, RobotMapPos targetPos, List<RobotMapPos> allRobotMapPos) {
        return List.of();
    }

    @Override
    public List<RobotMapPos> getRobotMapPos(String mapCode) {
        return List.of();
    }

    @Override
    public List<RobotMap> getMapList() {
        return List.of();
    }

    @Override
    public String getCurrentMap() {
        return "";
    }

    @Override
    public MapAttributesDto getMapAttributes(String mapCode) {
        return null;
    }

    @Override
    public void selMap(RobotMap map) {
    }

    @Override
    public PointDto canvas2Point(RobotMap robotMap, PointDto pointDto) {
        return null;
    }


    @Override
    public List<RobotCustomInfo> getCustomInfo() {
        List<RobotCustomInfo> customInfo = new ArrayList<RobotCustomInfo>();
        Object gfStatus =  GfCacheUtil.get(GfCacheUtil.KEY_ROBOT_STATUS);
        GfRobotStatus status = GfRobotStatus.builder().robotSerial("").reachStatus((short) 0).qdlSpeed(0)
                .gsSpeed(0).qdlMotorCurrent(0).gsMotorCurrent(0)
                .qdlMotorDis(0).gsMotorDis(0).qdlMotorErr(0).gsMotorErr(0)
                .power((short) 0).voltage(0).current(0).ratedCapacity(0)
                .temperature((short) 0).currentDirection((short) 0).directionInfo((short) 0)
                .workNum((byte) 0).workStatus((byte) 0).psStatus((byte) 0).psErrStatus((byte) 0)
                .chipTemperature((byte) 0).psRunTime(0).psOpenStatus((byte) 0).forwardStatus((byte) 0)
                .psSetTime(0).leftProximitySwitchTriggeringStatus((byte) 0)
                .rightProximitySwitchTriggeringStatus((byte) 0).robotSpeed(0)
                .reverseStatus((byte) 0).cameraAndWiperEnable((byte)0).runningDriverEnable((byte)0)
                .gsDriverEnable((byte) 0).ipcEnable((byte) 0).build();
        if (gfStatus != null) {
            status = (GfRobotStatus) gfStatus;
        }
        GfRelayStatus relayStatus =(GfRelayStatus) GfCacheUtil.getLongtimeCache(GfCacheUtil.KEY_ROBOT_STATUS);
        if (relayStatus!=null){
            customInfo.add(RobotCustomInfo.create("relayStatus1", "继电器1状态", relayStatus.getStatus1()==1? "开启": "关闭"));
            customInfo.add(RobotCustomInfo.create("relayStatus2", "继电器2状态", relayStatus.getStatus2()==1? "开启": "关闭"));
        }
        customInfo.add(RobotCustomInfo.create("qdlSpeed", "驱动轮速度", String.valueOf(status.getQdlSpeed() + "(R/min)")));
        customInfo.add(RobotCustomInfo.create("gsSpeed", "滚刷速度", String.valueOf(status.getGsSpeed() + "(R/min)")));
        customInfo.add(RobotCustomInfo.create("qdlMotorCurrent", "驱动轮电机电流", String.valueOf(status.getQdlMotorCurrent() + "(A)")));
        customInfo.add(RobotCustomInfo.create("gsMotorCurrent", "滚刷电机电流", String.valueOf(status.getGsMotorCurrent() + "(A)")));
        customInfo.add(RobotCustomInfo.create("qdlMotorDis", "驱动轮电机里程计", String.valueOf(status.getQdlMotorDis())));
        customInfo.add(RobotCustomInfo.create("gsMotorDis", "滚刷电机里程计", String.valueOf(status.getGsMotorDis())));
        customInfo.add(RobotCustomInfo.create("qdlMotorErr", "驱动轮电机故障状态", status.getQdlMotorErr() == 0 ? "正常": "故障码:" +  status.getQdlMotorErr()));
        customInfo.add(RobotCustomInfo.create("gsMotorErr", "滚刷电机故障状态", status.getQdlMotorErr() == 0 ? "正常": "故障码:" +  status.getGsMotorErr()));
        customInfo.add(RobotCustomInfo.create("workNum", "当前工作次数", String.valueOf(status.getWorkNum())));
        customInfo.add(RobotCustomInfo.create("chipTemperature", "芯片温度", String.valueOf(status.getChipTemperature())));
        customInfo.add(RobotCustomInfo.create("psStatus", "光电传感器状态", status.getPsStatus() == 0 ? "未触发": "触发"));
        customInfo.add(RobotCustomInfo.create("psErrStatus", "光电传感器堵转状态", status.getPsErrStatus() == 0 ? "未触发": "触发"));
        customInfo.add(RobotCustomInfo.create("psOpenStatus", "光电避障开关状态", status.getPsOpenStatus() == 1 ? "开启": "关闭"));
        customInfo.add(RobotCustomInfo.create("leftProximitySwitchTriggeringStatus", "左接近开关状态", status.getLeftProximitySwitchTriggeringStatus() == 0 ? "触发": "未触发"));
        customInfo.add(RobotCustomInfo.create("rightProximitySwitchTriggeringStatus", "右接近开关状态", status.getRightProximitySwitchTriggeringStatus() == 0 ? "触发": "未触发"));
        customInfo.add(RobotCustomInfo.create("cameraAndWiperEnable", "雨刷与相机使能状态", status.getCameraAndWiperEnable() == 1 ? "使能": "未使能"));
        customInfo.add(RobotCustomInfo.create("runningDriverEnable", "行走电机使能状态", status.getRunningDriverEnable() == 1 ? "使能": "未使能"));
        customInfo.add(RobotCustomInfo.create("gsDriverEnable", "滚刷电机使能状态", status.getGsDriverEnable() == 1 ? "使能": "未使能"));
        customInfo.add(RobotCustomInfo.create("ipcEnable", "工控机使能状态", status.getIpcEnable() == 1 ? "使能": "未使能"));
        customInfo.add(RobotCustomInfo.create("mileage","编码器编码值",String.valueOf(status.getMileage())));
        return customInfo;
    }



    @Override
    public FormConfigDto getRobotConfig(String formName) {
        // 创建字段配置
        ArrayList<FormFieldConfigDto> fields = new ArrayList<>();
        // 传感器使能
        fields.add(RobotParamsFormFieldEnum.ULTRASONIC_ENABLE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.COLLISION_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.DROP_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.INFRARED_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.SAFETY_AVOIDANCE_ENABLED.toConfigDto());
        // 传感器参数
        fields.add(RobotParamsFormFieldEnum.FRONT_ULTRASONIC_TRIGGER_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.FRONT_ULTRASONIC_RELEASE_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.BACK_ULTRASONIC_TRIGGER_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.BACK_ULTRASONIC_RELEASE_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.CHARGER_IP.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.CHARGER_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RELAY_IP.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RELAY_PORT.toConfigDto());
        // 机器人参数
        fields.add(RobotParamsFormFieldEnum.ROBOT_SPEED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.ROBOT_MILEAGE_CALIBRATION_FACTOR.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RFID_READER_POWER.toConfigDto());
        // 通过 FormConfigEnum 创建表单配置
        return FormConfigEnum.ROBOT_PARAMS_CONFIG.toFormConfigDto(fields);
    }

    @Override
    public RobotDriverEnum getDriverVersion() {
        return RobotDriverEnum.GF;
    }

    public static void main(String[] args) {
        HashMap<Object, Object> map = new HashMap<>();
        map.put("a", 1);
        map.put("a", 1);
        System.out.println(JSONObject.toJSONString(map));
        System.out.println("{\"number\": \"1\",\"enable\": \"2\"}");
    }

}
